/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/*LINTLIBRARY*/
/*PROTOLIB1*/

#include "main.h"
/*#include <math.h>*/
#include <stdio.h>
#include "delay.h"
#include "celp_sup.h"
#include "resample.h"
#include <assert.h>

#if 0  /* now implemented in its own file */
STATIC void Resample(
fxpt_16 sig_in[],
fxpt_16 wsinc[MAX_M1+MAX_M2+1][MAX_NFRAC],
int     q_frac_pit,
int     out_len,
int     out_start,
int     int_pit,
int     m1,
int     m2,
fxpt_16 sig_out[]);
#endif


STATIC void GenHam(
int     m1,
int     m2,
fxpt_16 wsinc[MAX_M1+MAX_M2+1][MAX_NFRAC]);

STATIC int quan_frac(fxpt_16    frac_pit);

fxpt_16 sinc(fxpt_16 arg);



/* 5.10 format */
static const fxpt_16 delay_frac[MAX_NFRAC] = {256, 341, 512, 683, 768};


/*************************************************************************
*
* ROUTINE
*               delay
*
* FUNCTION
*               Time delay a bandlimited signal of either 8 or 40 points
*               using point-by-point recursion
*
* SYNOPSIS
*               delay(SigIn, SigInLen, SigOutStart, SigOutLen, FracPit,
*                       IntPit, IntLow, IntUp, NumFracDelay, SigOut)
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       SigIn           fxpt_16 i/o     signal input (output in last 60)
*       SigInLen        int      i      length of input sequence x(n)
*       SigOutStart     int      i      beginning of output sequence
*       SigOutLen       int      i      length of output sequence
*       FracPit         fxpt_16  i      fractional pitch
*       IntPit          int      i      integer pitch
*       IntLow          int      i      Lower interpolation bound
*       IntUp           int      i      Upper interpolation bound
*       NumFracDelay    int      i      Number of fractional delays
*       SigOut          fxpt_16  o      delayed input signal
*==========================================================================
*
* DESCRIPTION
*
*       Subroutine to time delay a bandlimited signal by resampling the
*       reconstructed data (aka sinc interpolation).  The well known
*       reconstruction formula is:
*
*              |    M2      sin[pi(t-nT)/T]    M2
*   y(n) = X(t)| = SUM x(n) --------------- = SUM x(n) sinc[(t-nT)/T]
*              |   n=M1         pi(t-nT)/T    n=M1
*              |t=n+d
*
*       The interpolating (sinc) function is Hamming windowed to bandlimit
*       the signal to reduce aliasing.
*
*       Multiple simultaneous time delays may be efficiently calculated
*       by polyphase filtering.  Polyphase filters decimate the unused
*       filter coefficients.  See Chapter 6 in C&R for details.
*==========================================================================
*
* REFERENCES
*
*       Crochiere & Rabiner, Multirate Digital Signal Processing,
*       P-H 1983, Chapters 2 and 6.
*
*       Kroon & Atal, "Pitch Predictors with High Temporal Resolution,"
*       ICASSP '90, S12.6
*
***************************************************************************/

void delay_short(
fxpt_16 SigIn[],                /* 15.0 format */
fxpt_16 FracPit,                /* 8.7 format */
int     IntPit,
fxpt_16 SigOut[])
{
        static int      ShortFirst=TRUE;
        static fxpt_16  Shortwsinc[MAX_M1+MAX_M2+1][MAX_NFRAC];
        int             q_frac_pit;

        /* Generate appropriate Hamming windowed sinc interpolating functions */
        if (ShortFirst) {
                ShortFirst = FALSE;

                /*  Generate Hamming windowed sinc interpolating
                 *  function for each allowable fraction
                 */
                GenHam(-4, 3, Shortwsinc);
        }

        /*  Quantize the fractional pitch */
        q_frac_pit = quan_frac(FracPit);


        /*  Resample */
        Resample_short(SigIn, Shortwsinc, q_frac_pit, IntPit, SigOut);
}


void delay_long(
fxpt_16 SigIn[],                /* 15.0 format */
fxpt_16 FracPit,                /* 8.7 format */
int     IntPit,
fxpt_16 SigOut[])
{
        static int      LongFirst=TRUE;
        static fxpt_16  Longwsinc[MAX_M1+MAX_M2+1][MAX_NFRAC];
        int             q_frac_pit;

        /* Generate appropriate Hamming windowed sinc interpolating functions */
        if (LongFirst) {
                LongFirst = FALSE;

                /*  Generate Hamming windowed sinc interpolating
                 *  function for each allowable fraction
                 */
                GenHam(-20, 19, Longwsinc);
        }

        /*  Quantize the fractional pitch */
        q_frac_pit = quan_frac(FracPit);


        /*  Resample */
        Resample_long(SigIn, Longwsinc, q_frac_pit, IntPit, SigOut);
}



/*************************************************************************
*
* ROUTINE
*               GenHam
*
* FUNCTION
*
*               Generate Hamming windowed sinc interpolating function
*
* SYNOPSIS
*               GenHam(nfrac, frac, m1, m2, twelfths, wsinc, hwin)
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       nfrac           int      i      number of fractional delays
*       frac            fxpt_16  i      "nfrac" fractional delays calculated
*                                       over interpolation of M1 to M2
*       m1              int      i      Lower interpolation bound
*       m2              int      i      Upper interpolation bound
*       twelfths        int      i      ???
*       wsinc           fxpt_16  o      Hamming windowed sinc interpolating
*                                       function
*       hwin            fxpt_16  o      Hamming window
**************************************************************************
*
*       Generate Hamming windowed sinc interpolating function
*       for each allowable fraction.  The interpolating functions
*       are stored in time reverse order (i.e., delay appears as
*       advance) to align with the adaptive code book v0 array.
*       The interp filters are:
*               wsinc(.,1)      frac = 1/4 (3/12)
*               wsinc(.,2)      frac = 1/3 (4/12)
*               .               .
*               wsinc(.,5)      frac = 3/4 (9/12)
*
**************************************************************************/

void GenHam(
int     m1,
int     m2,
fxpt_16 wsinc[MAX_M1+MAX_M2+1][MAX_NFRAC])      /* 0.15 format */
{
        int     i,j;
        fxpt_16 hwin[MAX_WINDOW_WIDTH+1];       /* 0.15 format */
        int     twelfths[MAX_NFRAC] = {3,4,6,8,9};

        CreateHam(hwin, 12*(m2-m1+1)+1);

        for(i=0; i<MAX_NFRAC; i++) {
                for(j=m1; j<m2+1; j++) {
                        wsinc[(j-m1)][i] = sinc(fxpt_add16(delay_frac[i],
                            fxpt_shl16_fast(j, 10)));
                        wsinc[(j-m1)][i] = fxpt_mult16_round(wsinc[j-m1][i],
                            hwin[12 * (j-m1) + twelfths[i]], 15);
                }
        }
}


/*************************************************************************
*
* ROUTINE
*               quan_frac
*
* FUNCTION
*
*               Quantize the fractional pitch
*
* SYNOPSIS
*               q_frac_pit = quan_frac(frac_pit, nfrac, frac);
*
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       frac_pit        fxpt_16  i      Fractional pitch
*       nfrac           int      i      Number of fractional delays
*       frac            fxpt_16  i      "nfrac" fractional delays calculated
*                                       over interpolation of m1 to m2
*
*       q_frac_pit      int      o      Quantized fractional pitch
**************************************************************************/

int quan_frac(
fxpt_16 frac_pit                /* 8.7 format */
)
{
        /*int   ok=FALSE; */
        int     i, k;
        fxpt_16 fp = fxpt_shl16_fast(frac_pit, 3);
        fxpt_16 dist = 0, last;

        last = fxpt_abs16(fxpt_sub16(fp, delay_frac[0]));
        for (i=1; i<MAX_NFRAC; i++) {
                dist = fxpt_abs16(fxpt_sub16(fp, delay_frac[i]));
                if (dist > last)
                        break;
                last = dist;
        }
        k = i-1;

        if (last > 10) {
                /* This should not occur.  The reason this code has been
                 * added is to compensate for faulty interpolation in error
                 * correction of corrupted fractional pitch indexes.
                 */
                CELP_PRINTF(("delay.c: warning: large error (%d) in quantized fractional pitch\n", dist));
        }

if (k < 0 || k > MAX_NFRAC) {
        CELP_PRINTF(("quan_frac is out of bounds (%d)\n", k));
        CELP_ABORT();
}

        return k;
}

/*************************************************************************
*
* ROUTINE
*               Resample
*
* FUNCTION
*
*               Upsample and select appropriate samples to resample
*               at fractional delay
*
* SYNOPSIS
*
*               Resample(sig_in, wsinc, q_frac_pit, out_len,
*                       out_start, int_pit, m1, m2, sig_out)
*
*
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       sig_in          fxpt_16  i      Input signal
*       wsinc           fxpt_16  i      Hamming windowed sinc interpolating
*                                       function
*       q_frac_pit      int      i      Quantized fractional pitch
*       out_len         int      i      Length of output signal
*       out_start       int      i      Beginning of output signal
*       int_pit         int      i      Integer pitch
*       m1              int      i      Lower interpolation bound
*       m2              int      i      Upper interpolation bound
*       sig_out         fxpt_16  o      Delayed input signal
**************************************************************************/
#if 0 /* function now implemented in its own file */
void Resample(
fxpt_16 sig_in[],                               /* 15.0 format */
fxpt_16 wsinc[MAX_M1+MAX_M2+1][MAX_NFRAC],      /* 0.15 format */
int     q_frac_pit,
int     out_len,
int     out_start,
int     int_pit,
int     m1,
int     m2,
fxpt_16 sig_out[])                              /* 15.0 format */
{
        int     i, j;

        for (i=0; i<out_len; i++) {
                sig_in[out_start + i-1] = 0;
                for (j=m1; j<m2+1; j++) {
                        /*sig_in[out_start + i-1] += sig_in[out_start - int_pit + i + j -1] * wsinc[j-m1][q_frac_pit];*/

                        sig_in[out_start + i-1] = fxpt_add16(
                            sig_in[out_start + i-1],
                            fxpt_mult16_fast(
                                sig_in[out_start - int_pit + i + j-1],
                                wsinc[j-m1][q_frac_pit]));
                }
        }

        for(i=0; i<out_len; i++)        {
                sig_out[i] = sig_in[out_start + i - 1];
                sig_in[out_start+i-1] = 0;
        }
}
#endif


/*************************************************************************
*
* ROUTINE
*               sinc
*
* FUNCTION
*
*               Calculate sinc funtion of input
*
* SYNOPSIS
*               sinc_arg = sinc(arg);
*
*
*   formal
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       arg             fxpt_16  i      Argument
*
*       sinc_arg        fxpt_16  o      sin(pi*arg) / pi*arg
**************************************************************************/

fxpt_16 sinc(fxpt_16 arg)       /* arg is in 5.10 format */
{
        fxpt_32 num;
        fxpt_16 sinc_arg;
        fxpt_16 pi, den, omega;

        /* sigh */
        arg = arg > 20860 ? 20860 : arg < -20860 ? -20860 : arg;

        pi = 3217;              /* pi in 5.10 format */
        den = fxpt_mult16_fix(arg, pi, 11);     /* den in 6.9 format */

        /* Normalize omega to [0..2] (gradians) */
        omega = arg;
        if (omega >= 2048)
                omega = arg & 0x7ff;            /* omega = omega % 2048 */
        else if (omega < -2048) {
                omega = fxpt_negate16(omega);
                omega = omega & 0x7ff;          /* omega = omega % 2048 */
                omega = fxpt_negate16(omega);
        }
        if (omega >= 1024)
                omega = fxpt_sub16_fast(omega, 2048);
        else if (omega < -1024)
                omega = fxpt_add16_fast(omega, 2048);

        /* Convert omega to 0.15 format */
        omega = fxpt_shl16_fast(omega, 5);

        if (arg == 0)
                sinc_arg = 32767;       /* approx 1.0 in 0.15 format */
        else {
                /*sinc_arg = (float)(sin((double)(pi * arg)) / (pi * arg));*/
                num = fxpt_deposit_h(fxpt_sine16(omega));
                sinc_arg = fxpt_extract_l(fxpt_shr32_fast(num/den, 7));
        }

        return sinc_arg;        /* 0.15 format */
}
